
#include <assert.h>
#include <cstdio>
#include <algorithm>
#include "Polygon.h"
#include <iostream>
#include <unordered_set>

using navmesh::Polygon;

Polygon::Polygon(double pos[], int size)
{
	assert(size > 10);
	assert(size < MAXPOINT * 2);
	for (int i = 0; i < size; i += 2)
	{
		// 生成多边形的点集，两两为一组
		int x = pos[i];
		int y = pos[i + 1];
		points.push_back(Point(x, y));
	}
	// 对多边形进行Delaunay三角剖分
	Delaunay();
	// 生成、修正额外数据
	GenExtData();
}

void Polygon::CreateTriangle(Hash *eindexs, int p1, int p2, int p3)
{
	triangles.push_back(Triangle(p1, p2, p3));
	int triangle = triangles.size() - 1;
	// 构建3条边（若已有则不重复构建），约束边也在此被一并构建到Polygon.edges中
	triangles[triangle].edges[0] = CreateEdge(eindexs, triangle, p1, p2); // 点的顺序要保持方向，外层逆时针，内层顺时针
	triangles[triangle].edges[1] = CreateEdge(eindexs, triangle, p3, p2);
	triangles[triangle].edges[2] = CreateEdge(eindexs, triangle, p1, p3);
}

int Polygon::CreateEdge(Hash *eindexs, int triangle, int p1, int p2)
{
	// 这条边不在目前的边集中
	int k = PIndex(p1, p2);
	if (eindexs->find(k) == eindexs->end())
	{
		// 新构建1条边并加入到边集中
		int v = edges.size();
		edges.push_back(Edge(triangle, -1, p1, p2));
		eindexs->insert(make_pair(k, v));
		return v;
	}

	// 已经在边集中
	// 划分而形成的边，是允许作为2个三角形的共边的
	int v = (*eindexs)[k];
	int t2 = edges[v].triangles[1];
	assert(t2 < 0);
	edges[v].triangles[1] = triangle;
	return v;
}

// 叉积ab×ac，ac在ab的哪一侧，c点相对于ab的位置
double mult(Point a, Point b, Point c)
{
	return (a.x - b.x) * (a.y - c.y) - (a.y - b.y) * (a.x - c.x);
}

#define max(a, b) ((a) > (b) ? (a) : (b))
#define min(a, b) ((a) > (b) ? (b) : (a))
// aa, bb为一条线段两端点 cc, dd为另一条线段的两端点 相交返回true, 不相交返回false
bool intersect(Point aa, Point bb, Point cc, Point dd)
{
	// 线段1的最右端点 在 线段2最左端点的 左边
	if (max(aa.x, bb.x) < min(cc.x, dd.x))
	{
		return false;
	}
	// 线段1的最顶端点 在 线段2最底端点的 左边
	if (max(aa.y, bb.y) < min(cc.y, dd.y))
	{
		return false;
	}
	// 反向检测
	if (max(cc.x, dd.x) < min(aa.x, bb.x))
	{
		return false;
	}
	if (max(cc.y, dd.y) < min(aa.y, bb.y))
	{
		return false;
	}
	// cd在ab同侧
	if (mult(aa, bb, cc) * mult(aa, bb, dd) > 0)
	{
		return false;
	}
	// ab在cd同侧
	if (mult(cc, dd, aa) * mult(cc, dd, bb) > 0)
	{
		return false;
	}
	// c点位于ab上的情况也被认为是相交，要更正这一点的话将叉积相乘的判断改为>=0即可

	return true;
}

// 根据点乘公式计算first-center-second的角度（弧度制）
static inline double Angle(Point cen, Point first, Point second)
{
	double dx1, dx2, dy1, dy2;

	dx1 = first.x - cen.x;
	dy1 = first.y - cen.y;
	dx2 = second.x - cen.x;
	dy2 = second.y - cen.y;
	// 求（cen 到 first 的距离）*（cen 到 second 的距离）
	double c = (double)sqrt(dx1 * dx1 + dy1 * dy1) * (double)sqrt(dx2 * dx2 + dy2 * dy2);
	// 避免 cen与  first  或者 second  重叠的现象
	if (c == 0)
		return 0;

	return (double)acos((dx1 * dx2 + dy1 * dy2) / c);
}

static inline int lerp(int v, int min, int max)
{
	if (v < min)
		v = min;
	if (v > max)
		v = max;
	return v;
}

// 对边p1p2找它的Delaunay点p3，构成最优的DT三角形
int Polygon::FindDT(Grid *grid, int p1, int p2)
{
	// 定位到边p1p2的中点
	double x1 = points[p1].x, y1 = points[p1].y;
	double x2 = points[p2].x, y2 = points[p2].y;
	double x = (x1 + x2) / 2, y = (y1 + y2) / 2;

	// 中点对应哪个grid格
	double minx = grid->minx, miny = grid->miny;
	double gride_len = grid->gride_len;
	int gx = (int)((x - minx) / gride_len);
	int gy = (int)((y - miny) / gride_len);
	// 边界
	int xnum = grid->xnum, ynum = grid->ynum;

	int d = 0;
	int p3 = -1;
	double angle3 = -1;
	Point point1 = GetPoint(p1), point2 = GetPoint(p2);
	while (1)
	{
		// 从中点所在的grid开始螺旋式外扩，在经过的grid格中找可见点p3
		for (int i = -d; i <= d; i++)
		{
			for (int j = -d; j <= d; j++)
			{
				// pos定位到当前迭代的grid格
				int pos = lerp(gx + i, 0, xnum - 1) + lerp(gy + j, 0, ynum - 1) * xnum;
				if (pos >= 0 && pos < (int)grid->cells.size())
				{
					// 遍历grid格中的Point
					Cell c = grid->cells[pos];
					for (unsigned k = 0; k < c.points.size(); k++)
					{
						int p = c.points[k];
						Point point = GetPoint(p);
						if (p1 != p && p2 != p && (point2 - point1).inLeft(point - point1)) // p 要在p1p2的左侧
						{
							// 检查p与p1、p2是否可见，即它们的连线不能穿过任一约束边；我们检查连线与约束边是否相交即可判断
							bool flag = JudgeIsVisible(p1, p, grid) && JudgeIsVisible(p2, p, grid);
							// 通过可见性判定
							if (flag)
							{
								// angle = point1-point-point2所形成的角度，选择角度更大的作为DT点
								double angle = Angle(point, point1, point2);
								if (p3 == -1 || angle > angle3)
								{
									angle3 = angle;
									p3 = p;
								}
							}
						}
					}
				}
			}
		}

		// 判断是否应该结束当前趟
		if (p3 != -1)
		{
			// 求三角形p1p2p3的外接圆
			Circle c(point1, point2, GetPoint(p3));
			Point cc = c.GetCenter();
			double radius = c.GetR();
			// 求外接圆的grid包围盒（长方形、正方形）
			double l = cc.x - radius, r = cc.x + radius, t = cc.y - radius, b = cc.y + radius;
			int lx = lerp((int)((l - minx) / gride_len), 0, xnum - 1);
			int rx = lerp((int)((r - minx) / gride_len), 0, xnum - 1);
			int ty = lerp((int)((t - miny) / gride_len), 0, ynum - 1);
			int by = lerp((int)((b - miny) / gride_len), 0, ynum - 1);
			// 外接圆的包围盒，在外扩的已处理范围之内，那么最优的DT点已经找到了，结束循环
			if ((gx - d) <= lx && (gx + d) >= rx && (gy - d) <= ty && (gy + d) >= by)
				break;
		}
		d++;
	}
	assert(p3 != -1);
	return p3;
}

Point Polygon::GetPoint(int p)
{
	return points[p];
}

// pa1、p1的连线是否穿过edgepos对应grid格中的某条线段
bool Polygon::IsIntersect(Grid *grid, int edgepos, int pa1, int p1)
{
	Point pa = GetPoint(pa1);
	Point p = GetPoint(p1);

	// 要遍历edgepos代表的grid格的边集，这样子的处理让遍历范围缩小了很多，不用把地图上所有的边都遍历一遍
	Cell c = grid->cells[edgepos];
	for (unsigned m = 0; m < c.edges.size(); m++)
	{
		// Polygon()初始化的前序步骤里，我们默认了与eposId构成边的是eposId + 1
		int eposId = c.edges[m];
		int next_eposId = (eposId + 1) % points.size();

		// 相交检测
		bool flag = intersect(GetPoint(next_eposId), GetPoint(eposId), pa, p);
		// 两线段有共点端点，这种情况是相交，但不算“穿过”
		if ((eposId == pa1) || (eposId == p1) || (next_eposId == pa1) || (next_eposId == p1))
		{
			flag = false;
		}

		if (flag)
			return true;
	}
	return false;
}

// 检查pindex1与pindex2是否可见，即它们的连线不能穿过任一约束边；我们检查连线与约束边是否相交即可判断
bool Polygon::JudgeIsVisible(int pindex1, int pindex2, Grid *grid)
{
	Point p1 = GetPoint(pindex1);
	Point p2 = GetPoint(pindex2);
	double minx = grid->minx;
	double miny = grid->miny;
	int gride = grid->gride_len;
	int xnum = grid->xnum;
	int ynum = grid->ynum;

	// 规约p1是x坐标更小的点
	if (p1.x > p2.x)
	{
		Point demo = p2;
		p2 = p1;
		p1 = demo;
	}

	int xn1 = (int)((p1.x - minx) / gride);
	int yn1 = (int)((p1.y - miny) / gride);
	int xn2 = (int)((p2.x - minx) / gride);
	int yn2 = (int)((p2.y - miny) / gride);

	// 看p1p2的连线会经过哪些grid格，再找grid格绑定的所有边进行穿过检测

	// p1、p2在同一个gird列里
	if (xn1 == xn2)
	{
		if (yn1 > yn2)
		{
			yn1 = yn1 ^ yn2;
			yn2 = yn1 ^ yn2;
			yn1 = yn1 ^ yn2;
		}
		// 在y轴方向变大迭代grid格即可
		for (int j = yn1; j <= yn2; j++)
		{
			if (j > ynum)
				break;

			// 穿线检测
			int edgepos = (xn1 >= xnum ? xnum - 1 : xn1) + (j >= ynum ? ynum - 1 : j) * xnum;
			if (IsIntersect(grid, edgepos, pindex1, pindex2))
				return false;
		}
	}
	else
	{
		double x = p1.x;
		double y = p1.y;
		for (int i = xn1; i <= xn2; i++)
		{
			double x3 = minx + (i + 1) * gride;
			if (x3 > p2.x)
				x3 = p2.x;

			// 利用两点连线的斜率，计算出x3对应的y3
			// 两点求得方程 (y-y2)/(y1-y2)=(x-x2)/(x1-x2)，把x3代入计算y3
			double y3 = (p2.x - x3) * (p2.y - p1.y) / (p1.x - p2.x) + p2.y;

			int cur_x = (int)((x - minx) / gride);
			int cur_y = (int)((y - miny) / gride);
			int next_y = (int)((y3 - miny) / gride);
			// 规约迭代的y3对应的grid格不要在起点之下
			if (next_y < cur_y)
			{
				cur_y = cur_y ^ next_y;
				next_y = cur_y ^ next_y;
				cur_y = cur_y ^ next_y;
			}
			// 定位到需要迭代的y轴上的grid格是哪些
			for (int j = cur_y; j <= next_y; j++)
			{
				if (j > ynum)
					break;
				// 穿线检测
				int edgepos = (cur_x >= xnum ? xnum - 1 : cur_x) + (j >= ynum ? ynum - 1 : j) * xnum;
				if (IsIntersect(grid, edgepos, pindex1, pindex2))
					return false;
			}
			x = x3;
			y = y3;
		}
	}
	return true;
}

void Polygon::Delaunay()
{
	/* 
		1. 构建合适的网格划分大小
		2. 将点、边，与对应的网格建立关联
		3. 随便从一条最外边开始，将整个多边形的内部划分为多个Delaunay三角。

		* （可选）检查最大最小值合法性
	*/

	// 找出点集中的minx, maxx, miny, maxy
	Point p0 = points[0];
	double minx = p0.x, miny = p0.y, maxx = p0.x, maxy = p0.y;
	for (auto const &it : points)
	{
		double x = it.x;
		double y = it.y;
		if (x < minx)
			minx = x;
		if (x > maxx)
			maxx = x;
		if (y < miny)
			miny = y;
		if (y > maxy)
			maxy = y;
	}

	// 算出相对均衡的正方形grid边长
	double dx = maxx - minx, dy = maxy - miny;
	int gride_len = (int)sqrt(dx * dy / points.size());
	// x轴上小grid个数，y轴上小grid个数
	int xnum = (int)ceil(dx / gride_len);
	int ynum = (int)ceil(dy / gride_len);

	grid.gride_len = gride_len;
	grid.minx = minx;
	grid.maxx = maxx;
	grid.miny = miny;
	grid.maxy = maxy;
	grid.xnum = xnum;
	grid.ynum = ynum;

	// 数组cells有xnum * ynum个槽位，xnum * ynum = 小grid的总数
	vector<Cell> cells(xnum * ynum);
	for (auto it = points.cbegin(); it != points.cend(); it++)
	{
		// point放到小grid里
		double x = it->x;
		double y = it->y;
		// point被放到了x、y轴上的第几个grid中
		int xn = (int)((x - minx) / gride_len);
		int yn = (int)((y - miny) / gride_len);
		// 每个grid都对应到唯一的posId
		int posId = (xn >= xnum ? xnum - 1 : xn) + (yn >= ynum ? ynum - 1 : yn) * xnum;
		// point = 在points中的index
		int point = it - points.cbegin();
		cells[posId].points.push_back(point);

		// edges放到Grid里

		// 顶点之间，按序号排列，相邻的两点之间有1条边，注意处理首个顶点和最后一个顶点之间的相连关系
		Point p = GetPoint(point);
		Point p1 = GetPoint((point + 1) % points.size());
		// p指代x坐标较小的Point
		if (p.x > p1.x)
		{
			Point demo = p;
			p = p1;
			p1 = demo;
		}

		// p、p1在grid图中的坐标
		int xn1 = (int)((p.x - minx) / gride_len);
		int yn1 = (int)((p.y - miny) / gride_len);
		int xn2 = (int)((p1.x - minx) / gride_len);
		int yn2 = (int)((p1.y - miny) / gride_len);

		if (xn1 == xn2)
		{
			if (yn1 > yn2)
			{
				// 交换yn1、yn2的数值
				yn1 = yn1 ^ yn2;
				yn2 = yn1 ^ yn2;
				yn1 = yn1 ^ yn2;
			}
			// j = yn1 ~ yn2
			for (int j = yn1; j <= yn2; j++)
			{
				int edgepos = (xn1 >= xnum ? xnum - 1 : xn1) + (j >= ynum ? ynum - 1 : j) * xnum;
				// 约束边被加入到cells中
				// 记录这条边经过的所有小grid，仅存储pointIdx，我们知道与它构成边的是pointIdx + 1即可
				cells[edgepos].edges.push_back(point);
			}
		}
		else
		{
			// 将x值偏小的y值保存起来
			double y = p.y;
			double x = p.x;
			for (int i = xn1; i <= xn2; i++)
			{
				// x3 = 第i个小grid的右边界x坐标
				double x3 = (i + 1) * gride_len + minx;
				// 修正x3最大等于p1.x
				if (x3 > p1.x)
					x3 = p1.x;

				// 两点求得方程 (y-y2)/(y1-y2)=(x-x2)/(x1-x2)，把x3代入计算y3, 方程是边的方程
				// 知道直线方程后，分别把格子两端的x值代入方程，就可以知道直线穿过的y方向的格子
				double y3 = (p.x - x3) * (p.y - p1.y) / (p1.x - p.x) + p.y;

				int cur_x = (int)((x - minx) / gride_len);
				int cur_y = (int)((y - miny) / gride_len);	 // 格子左侧y点坐标
				int next_y = (int)((y3 - miny) / gride_len); // 格子右侧y点坐标
				if (cur_y > next_y)
				{
					cur_y = cur_y ^ next_y;
					next_y = cur_y ^ next_y;
					cur_y = cur_y ^ next_y;
				}
				// 记录x前进一格，线段在y方向上跨越的格子数
				for (int j = cur_y; j <= next_y; j++)
				{
					if (j > ynum)
						break;
					int edgepos = (cur_x >= xnum ? xnum - 1 : cur_x) + (j >= ynum ? ynum - 1 : j) * xnum;
					// 记录这条边经过的所有小grid
					cells[edgepos].edges.push_back(point);
				}
				x = x3;
				y = y3;
			}
		}
	}

	grid.cells = cells;

	Hash eindexs; // eindexs[PIndex(p1, p2)] = 这条边在edges中的index，存放着所有已经处理过的三角形的边

	Hash restrains; // 标记着所有的约束边，即内外边界的边
	// PIndex(pindex1, pindex2) = 两点构成的边的唯一id = pindex1 00000 pindex2
	// restrains[PIndex(pindex1, pindex2)] = 1
	for (unsigned i = 0; i < points.size() - 1; i++)
	{
		restrains.insert(make_pair((int)PIndex(i, i + 1), 1));
	}
	restrains.insert(make_pair((int)PIndex(0, points.size() - 1), 1));

	vector<int> es; // 栈，暂存待处理的边
	// 从随便一条最外边开始
	int p1 = 0, p2 = 1;
	int e = PIndex(p1, p2);
	while (1)
	{
		// 对边p1p2找它的DT点p3，构成最优的DT三角形
		int p3 = FindDT(&grid, p1, p2);

		// 边p1p3不是原来就有的（最外）约束边之一
		if (restrains.find((int)PIndex(p1, p3)) == restrains.end())
		{
			// 边p1p3不在栈es中，则入栈；反之出栈；
			vector<int>::iterator it = std::find(es.begin(), es.end(), PIndex(p1, p3));
			// 边p1p3不在栈es中，则入栈；反之出栈；
			if (it == es.end())
			{
				es.push_back(PIndex(p1, p3));
			}
			else
			{
				es.erase(it);
			}
		}

		// 对边p2p3做同样的处理
		if (restrains.find((int)PIndex(p2, p3)) == restrains.end())
		{
			vector<int>::iterator it = std::find(es.begin(), es.end(), PIndex(p2, p3));
			if (it == es.end())
			{
				es.push_back(PIndex(p2, p3));
			}
			else
			{
				es.erase(it);
			}
		}

		// 创建三角形，构建边obj
		CreateTriangle(&eindexs, p1, p2, p3);

		// 已经没有待处理的边，三角形网格化停止
		if (es.empty())
			break;
		// 迭代至下一条待处理的边
		e = es.back();
		es.pop_back();
		int *points = edges[eindexs[e]].points;
		p1 = points[0], p2 = points[1];
	}
}

#define distance(p1, p2) ((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y))

void Polygon::GenExtData()
{
	for (unsigned i = 0; i < triangles.size(); i++)
	{
		// 生成三角形的重心、包围盒
		Triangle & t = triangles[i];
		t.GenExtData(this);
	}

	/*
		进行完三角剖分后，我们知道每条导航边都会与2个可行走三角形相关联（相邻）；每条边都包含两个端点，Point1、Point2；
		我们取导航边关联的第1个三角形的重心，并作 向量(重心->Point1) 和 向量(重心->Point2)，比较两向量的位置，令边Obj的Point1存储左端点，Point2存储右端点；
		这样一来，对于导航边关联的第2个三角形来说，边Obj的Point1，就是从内部向导航边移动时的右端点，边Obj的Point2就是左端点。
		执行漏斗算法时，我们知道当前迭代的是哪个三角形和导航边，判断一下当前三角形是导航边关联的第1还是第2个三角形，我们就可以快速得出在当前的前进方向上，导航边上的左右端点分别是哪个了。
	*/
	for (unsigned i = 0; i < edges.size(); i++)
	{
		Edge & e = edges[i];
		int e0 = e.triangles[0];
		int e1 = e.triangles[1];
		// 这条边被两个三角形共用，是内边（导航边）
		if (e0 >= 0 && e1 >= 0)
		{
			// 该边关联的第1个三角形
			Triangle t0 = triangles[e0];
			// 边的两个端点
			int p0 = e.points[0], p1 = e.points[1];

			// 前进方向 约等于 重心->导航边
			Point pp0 = GetPoint(p0) - t0.icenter; // 向量(重心 -> p0)
			Point pp1 = GetPoint(p1) - t0.icenter; // 向量(重心 -> p1)
			// 若pp1在pp0的左侧，则调换两个端点在Edge.points中的存储位置
			if (pp0.inLeft(pp1))
			{
				// 令边的points[0]存储该前进方向的左端点，边的points[1]存储该前进方向的右端点
				e.points[0] = p1;
				e.points[1] = p0;
			}
		}
	}
}

vector<Line> Polygon::GetLines()
{
	vector<Line> lines(edges.size());
	for (unsigned i = 0; i < edges.size(); i++)
	{
		Line line;
		line.p1 = GetPoint(edges[i].points[0]);
		line.p2 = GetPoint(edges[i].points[1]);
		if (edges[i].IsRestrain(this))
		{
			line.color[0] = 1.0;
			line.color[1] = 0.0;
			line.color[2] = 0.0;
		}
		else
		{
			line.color[0] = 0.0;
			line.color[1] = 1.0;
			line.color[2] = 0.0;
		}
		lines[i] = line;
	}
	return lines;
}

vector<Line> Polygon::GetGrideLines()
{
	vector<Line> lines(grid.ynum + grid.xnum + 2);
	// 画grid格竖向的线
	for (int i = 0; i <= grid.xnum; i++)
	{
		Line line;
		Point pa1, pa2;
		double x = i * grid.gride_len + grid.minx;
		pa1.x = x;
		pa1.y = grid.miny;

		pa2.x = x;
		pa2.y = grid.miny + grid.ynum * grid.gride_len;
		line.p1 = pa1;
		line.p2 = pa2;

		line.color[0] = 0.2f;
		line.color[1] = 0.3f;
		line.color[2] = 0.3f;

		lines[i] = line;
	}
	// 画grid格横向的线
	for (int j = 0; j <= grid.ynum; j++)
	{
		Line line;
		Point pa1, pa2;
		double y = j * grid.gride_len + grid.miny;
		pa1.x = grid.minx;
		pa1.y = y;
		pa2.x = grid.minx + grid.xnum * grid.gride_len;
		pa2.y = y;
		line.p1 = pa1;
		line.p2 = pa2;

		line.color[0] = 0.2f;
		line.color[1] = 0.3f;
		line.color[2] = 0.3f;

		lines[j + grid.xnum + 1] = line;
	}
	return lines;
}

vector<Point> Polygon::GetCenters()
{
	vector<Point> centers(triangles.size());
	for (unsigned i = 0; i < triangles.size(); i++)
	{
		centers[i] = triangles[i].icenter;
	}
	return centers;
}

// 遍历所有的三角形，判断p点在哪个三角形内部
int Polygon::FindTriangle(Point p)
{
	for (unsigned i = 0; i < triangles.size(); i++)
	{
		if (triangles[i].Contain(this, p))
			return i;
	}
	return -1;
}

// 找路径 from是起点，to是终点
vector<Point> Polygon::FindPath(Point from, Point to)
{
	std::cout << "======================== find path ========================" << std::endl;
	int tfrom = FindTriangle(from);
	int tto = FindTriangle(to);

	vector<int> ts; // 栈，存放要进行扩展搜索的三角形
	vector<int> es; // A*或其他寻路方法，找到的路径，所经过的所有边，用于拐点算法，进行路径优化
	vector<Point> ways;
	if (tfrom < 0)
		return ways;
	if (tto < 0)
		return ways;
	ways.push_back(from);
	if (tfrom == tto)
	{
		ways.push_back(to);
		return ways;
	}
	ts.push_back(tfrom);

	// A* F = G() + H()
	// 这里用的是贪心算法，令G() = 0，H() = 当前点到终点的距离（评估）
	int start = tfrom;

	Hash visited; // 标记该三角形在A*寻路中是否已经访问过
	visited.insert(make_pair(tfrom, 1));

	while (!ts.empty())
	{
		// 访问栈顶的三角形
		start = ts.back();
		double weight = -1;
		int next = -1;
		int e;
		// 遍历三角形的3条边
		for (int i = 0; i < 3; i++)
		{
			int eindex = triangles[start].edges[i];
			Edge edge = edges[eindex];
			// nextt = 该边对应的另一个三角形
			int nextt = (edge.triangles[0] == start) ? edge.triangles[1] : edge.triangles[0];
			// 如果这是一条约束边，那么另一个三角形其实是不存在的
			if (nextt >= 0)
			{
				// 该边对应的另一个三角形就是目标三角形，那么寻路该结束了
				if (nextt == tto)
				{
					next = tto;
					e = eindex;
					break;
				}
				// 这个三角形没进行过扩展探索
				if (visited.find(nextt) == visited.end())
				{
					// 该三角形的重心 到 目标点 的距离的平方
					// 在至多三个邻接三角形中，找到预计离目标点最近的那个
					Point p = triangles[nextt].icenter;
					double d = distance(p, to);
					if (next < 0 || weight > d)
						next = nextt, weight = d, e = eindex;
				}
			}
		}

		// 找到的next三角就是目标，甭继续找了
		if (next == tto)
		{
			ts.push_back(next);
			es.push_back(e);
			break;
		}
		// 将next三角入栈，等下一步对它进行扩展搜索，并且标记它已经扩展搜索过
		else if (next >= 0)
		{
			visited.insert(make_pair(next, 1));
			ts.push_back(next);
			es.push_back(e); // 记录这条边是当前路径要经过的边
		}
		// 进入到一个死胡同，与之相邻的方向要么已经探索过，要么是阻挡
		// 则将当前迭代的三角形出栈，对应的经过边也出栈
		// 回溯迭代到上一个三角形，继续去未曾搜索过的方向进行查找
		else
		{
			ts.pop_back();
			es.pop_back();
		}
	}

	// 没有路径
	if (ts.empty())
		return ways;

	// 哪些三角形被经过了
	for (int i = 0; i < triangles.size(); i++)
	{
		triangles[i].isPath = false;
	}
	for (int i : ts)
	{
		triangles[i].isPath = true;
	}

	// ===== 拐点算法 =====
	// main variables:
	// e  ep_l  ep_r   p_l  p_r   line_l  line_r  t  t_l  t_r
	// ne nep_l nep_r  np_l np_r

	// t代表漏斗的顶点在第t个三角形内部
	// t的取值范围 = 0 ~ (导航边总数 - 1)，也就是说终点所在的三角形它不迭代
	int t = 0;
	int size = es.size();
	std::cout << "size = " << size << std::endl;

	// 每次循环就是要找到一个拐点，然后下次循环是从新的拐点找下一个拐点
	while (t < size)
	{
		// ways中依次是：起点、拐点1、拐点2 ... 终点，初始情况下只有起点在里面
		// p是当前漏斗的顶点
		Point p = ways.back();
		printf("\n--------------------- 漏斗顶点cur_p(x=%f, y=%f)，漏斗顶点所在三角形t=%d ---------------------\n", p.x, p.y, t);

		// 漏斗顶点所在的三角形，朝向的导航边，可以确定漏斗的左右端点
		Edge e = edges[es[t]];
		int ep_r, ep_l;
		// ts[t]表示当前要处理的三角，判断它是否是导航边关联的第1个三角，由此确定出左右端点
		// 我们在初始化，三角剖分完成后处理过
		// 边的points[0] 是 第1个三角的重心向导航边移动时 的左端点，points[1]则是右端点。
		if (ts[t] == e.triangles[0])
		{
			ep_r = e.points[1], ep_l = e.points[0];
		}
		else
		{
			ep_r = e.points[0], ep_l = e.points[1];
		}

		// 当前漏斗的左右端点，左右端点在漏斗初始化时，根据当前迭代的导航边t确定；并随漏斗的缩小、端点位置不变、漏斗边同向延长而更新，除此以外不会有变更
		Point curRightPoint = GetPoint(ep_r);
		Point curLeftPoint = GetPoint(ep_l);
		// 向量(p->右端点)、向量(p->左端点)，即当前漏斗的右拉线和左拉线
		Point line_r = curRightPoint - p;
		Point line_l = curLeftPoint - p;

		// i 恒等于 t+1，代表t的下一个三角形（导航边）
		int i = t + 1;

		// 这两个变量不是一定会和i一起同步变化的
		int t_r = t; // 最后一次更新右端点时，所在的三角形是哪个
		int t_l = t; // 最后一次更新左端点时，所在的三角形是哪个

		bool continueTag = false;
		// i从t的下一条导航边开始，一直迭代到最后一条导航边
		// 每一次迭代，左右端点至多仅有1个的位置会发生变化
		// 找到拐点时会提前跳出，因为此时的漏斗顶点需要重新确定
		for (; i < size; i++)
		{
			printf("\nnext判断的三角形序号i=%d\n", i);
			printf("cur_p_right(x=%f, y=%f)\n", curRightPoint.x, curRightPoint.y);
			printf("cur_p_left(x=%f, y=%f)\n", curLeftPoint.x, curLeftPoint.y);

			// 拿到下一条穿出边的左右端点
			Edge ne = edges[es[i]];
			int nep_r, nep_l;
			if (ts[i] == ne.triangles[0])
			{
				nep_r = ne.points[1], nep_l = ne.points[0];
			}
			else
			{
				nep_r = ne.points[0], nep_l = ne.points[1];
			}
			Point np_r = GetPoint(nep_r);
			Point np_l = GetPoint(nep_l);
			printf("next_p_right(x=%f, y=%f)\n", np_r.x, np_r.y);
			printf("next_p_left(x=%f, y=%f)\n", np_l.x, np_l.y);
			// 漏斗的下1左拉线和下1右拉线
			Point line_n_r = np_r - p;
			Point line_n_l = np_l - p;

			if (line_r.inRight(line_n_l) || line_r.inSame(line_n_l))
			{
				printf("向量(p->下1个左端点) 在 当前右拉线 的右边 或 同向，不再符合漏斗的形状，当前的右端点被确定为拐点\n");
				p = curRightPoint;
				ways.push_back(curRightPoint);

				// 拐点确定后，我们仅希望，漏斗顶点所在的三角形，迭代到右端点最后一次update时所在的三角形的 下一个三角形即可
				// 不需要做更多的跳过步骤或迭代，这样可以保证后续处理的正确性

				// 如果拐角处有很多个三角形，那么可能会导致下一个漏斗初始化时，漏斗的顶点 == 漏斗的右端点
				// 这样会令line_r等于0，被判定为与下一条左拉线同向，从而继续触发本分支，迭代到下一个三角形 ...
				// 直到后续无导航边可迭代，或漏斗的顶点 和 漏斗的右端点不再重合为止
				t = t_r = (t_r + 1);
				printf("t = t_r = %d\n", t_r);
				continueTag = true;
				break;
			}
			else if (line_r.inLeft(line_n_r) || line_r.inSame(line_n_r))
			{
				printf("向量(p->下1个右端点) 在 当前右拉线 的左侧 或 同向，更新右拉线，缩小漏斗\n");
				curRightPoint = np_r;
				line_r = line_n_r;

				t_r = i;
				printf("t_r=%d\n", t_r);
			} else {
				// 什么都不做，维持原状，导航边继续迭代到下一条导航边
				printf("向量(p->下1个右端点) 在 当前右拉线 的右侧，使得漏斗变大，我们不更新端点\n");
			}

			if (line_l.inLeft(line_n_r) || line_l.inSame(line_n_r))
			{
				printf("向量(p->下1个右端点) 在 当前左拉线 的左侧 或 同向，不再符合漏斗的形状，当前的左端点被确定为拐点\n");
				p = curLeftPoint;
				ways.push_back(curLeftPoint);

				t = t_l = (t_l + 1);
				printf("t = t_l=%d\n", t_l);
				continueTag = true;
				break;
			}
			else if (line_l.inRight(line_n_l) || line_l.inSame(line_n_l))
			{
				printf("向量(p->下1个左端点) 在 当前左拉线 的右侧 或 同向，更新左拉线，缩小漏斗\n");
				curLeftPoint = np_l;
				line_l = line_n_l;

				t_l = i;
				printf("t_l=%d\n", t_l);
			}
			else {
				printf("向量(p->下1个左端点) 在 当前左拉线 的左侧，使得漏斗变大，我们不更新端点\n");
			}
		}

		if (continueTag) {
			printf("漏斗顶点所在的三角形已变化，必须重新构建漏斗\n");
			continue;
		}

		// 后续已经没有next导航边可以迭代，意味着漏斗已经没办法再变化了
		if (i >= size - 1)
		{
			// 终点可能在当前左右拉线以外的位置，所以需要在这里检测，并添加拐点
			printf("\ngo in last deal, 当前漏斗顶点p(x=%f, y=%f)\n", p.x, p.y);
			printf("cur_p_right(x=%f, y=%f)\n", curRightPoint.x, curRightPoint.y);
			printf("cur_p_left(x=%f, y=%f)\n", curLeftPoint.x, curLeftPoint.y);

			Point line_p_goal = to - p;
			if (line_r.inRight(line_p_goal) || line_r.inSame(line_p_goal))
			{
				printf("向量(p->终点) 在 当前右拉线 的右边 或同向，当前的右端点被确定为拐点\n");
				ways.push_back(curRightPoint);
				t = t_r; // 之所以这里不写t = t_r + 1，是因为最后会t++
				printf("t = t_r = %d\n", t_r);
			}
			else if (line_l.inLeft(line_p_goal) || line_l.inSame(line_p_goal))
			{
				printf("向量(p->终点) 在 当前左拉线 的左边 或同向，当前的左端点被确定为拐点\n");
				ways.push_back(curLeftPoint);
				t = t_l;
				printf("t = t_l=%d\n", t_l);
			} else {
				printf("终点在当前漏斗之内，可以提前结束漏斗算法\n");
				ways.push_back(to);
				break;
			}

			if (t >= size - 1)
			{
				printf("除终点所在的三角形以外，路径上的所有三角形都已迭代完毕\n");
				printf("现在从最后一条导航边上出发，可以到达终点所在三角中的任意一点，所以添加终点到路径中，漏斗算法由随后的t++终止\n");
				ways.push_back(to);
			}

			// 触及到终点不能直接break，而是要t++迭代的原因：
			// 想象一下，现在有个火山形状的可行走区域（八字形），我们要在火山内部（八的内部）行走，八字的两撇上有若干个点，形成了若干条约束边
			// 我们要从火山口开始，走到火山内部的某个位置，然后你现在在山顶斜上方，导航边往下迭代，寻路的漏斗是扩大的。

			// 假设你的终点在正下方，那么终点在当前的漏斗之内，那么确实可以直接break，我们上面已经优化了。

			// 假设你的终点在山脚的最左边（八字左边那一撇的最左边），那么沿着火山的左侧，其实会有一堆拐点产生
			// 应对这种情况，在每次迭代next导航边时，你提前比较 next左拉线 和 当前顶点-终点拉线，也是没有意义的；
			// 想象一下火山下面是个倒影火山，然后又接上一个正立火山，从上到下依次是：八 - 倒立的八 - 八，一共3个八字形区域
			// 终点在第3个八最左侧的情况，提前比较终点反而会在第1个八当中产生多余的拐点，实际上要在第3个八中才开始往左侧拐
			// 所以只能让新漏斗迁移到第3个八的火山口位置，接着再往下慢慢迭代
			t++;
			printf("t = (t + 1) = %d\n", t);
		}
	}

	std::vector<Point> result;
	std::unordered_set<Point> seen;
	for (const auto& pi : ways) {
		// 我们的漏斗算法可能会将重复的拐点添加到ways中，所以要进行去重处理
		printf("pi(x=%f, y=%f)\n", pi.x, pi.y);
		if (seen.find(pi) == seen.end()) {
			seen.insert(pi);
			result.emplace_back(pi);
		}
	}

	return result;
}
